#pragma once
#include <atomic>
#include <vector>
#include <chrono>
#include <string>
#include <Eigen/Dense>
#include "industrial_robot_control/ethercat/master.h"

namespace industrial_robot_control {
namespace safety {

enum class SafetyLevel {
    NORMAL,        // 正常运行
    WARNING,       // 警告状态
    SAFE_OPERATE,  // 安全运行模式
    SAFE_STOP,     // 安全停止
    EMERGENCY_STOP // 紧急停止
};

struct EmergencyStopSignal {
    std::string name;
    uint16_t slave_idx;
    uint16_t sdo_index;
    uint8_t sdo_subindex;
    bool active;
    std::chrono::system_clock::time_point last_triggered;
};

struct SafetyZone {
    std::string name;
    std::vector<Eigen::Vector3d> boundary; // 多边形边界点
    bool is_forbidden; // true: 禁止进入, false: 允许进入
};

class SafetyMonitor {
public:
    SafetyMonitor(ethercat::Master& ethercat_master);
    ~SafetyMonitor() = default;
    
    // 初始化安全配置
    bool initialize(const std::string& config_path);
    
    // 周期性安全检查
    void checkSafety();
    
    // 获取当前安全级别
    SafetyLevel getCurrentLevel() const { return current_level_; }
    
    // 获取最后错误信息
    std::string getLastError() const { return last_error_; }
    
    // 急停相关
    bool triggerEmergencyStop(const std::string& reason);
    bool resetEmergencyStop();
    bool isEmergencyStopped() const { return current_level_ == SafetyLevel::EMERGENCY_STOP; }
    
    // 安全区域检查
    bool isInSafeZone(const Eigen::Vector3d& position) const;
    
private:
    ethercat::Master& ethercat_master_;
    std::atomic<SafetyLevel> current_level_{SafetyLevel::NORMAL};
    std::vector<EmergencyStopSignal> estop_signals_;
    std::vector<SafetyZone> safety_zones_;
    std::string last_error_;
    mutable std::mutex error_mutex_;
    
    // 碰撞检测
    bool checkCollision(const Eigen::Vector3d& current_pos, const Eigen::Vector3d& next_pos);
    
    // 急停信号处理
    void processEmergencyStopSignals();
    
    // 安全区域检查
    void checkSafetyZones();
    
    // 设置安全级别并记录错误
    void setSafetyLevel(SafetyLevel level, const std::string& error);
    
    // 点是否在多边形内
    bool isPointInPolygon(const Eigen::Vector3d& point, const std::vector<Eigen::Vector3d>& polygon) const;
};

} // namespace safety
} // namespace industrial_robot_control
